package com.acheli.utils.physics;


import net.minecraft.world.phys.Vec3;
import org.joml.Quaternionf;

public class PhysicsObject {
    private Vec3 currentPos = Vec3.ZERO;
    private Vec3 currentForce = Vec3.ZERO;
    private Vec3 currentVelocity = Vec3.ZERO;
    private Quaternionf currentRotate = new Quaternionf();

    private boolean disabled = false;
    private double mass = 0;
    public Vec3 getCurrentPos() {
        return currentPos;
    }

    public void setCurrentPos(Vec3 currentPos) {
        this.currentPos = currentPos;
    }
    public void addForce(Vec3 force) {
        currentForce.add(force);
    }
    public Vec3 getCurrentForce() {
        return currentForce;
    }

    public void setCurrentForce(Vec3 currentForce) {
        this.currentForce = currentForce;
    }

    public Vec3 getCurrentVelocity(Vec3 input) {
        if (disabled) {
            return input;
        }
        return currentVelocity;
    }
    public Vec3 getCurrentVelocity() {
        return currentVelocity;
    }

    public void setCurrentVelocity(Vec3 currentVelocity) {
        this.currentVelocity = currentVelocity;
    }

    public Quaternionf getCurrentRotate() {
        return currentRotate;
    }

    public void setCurrentRotate(Quaternionf currentRotate) {
        this.currentRotate = currentRotate;
    }

    public double getMass() {
        return mass;
    }

    public void setMass(double mass) {
        this.mass = mass;
    }

    /**
     * 获取当前运动轨迹的切线方向单位向量
     * （切线方向即速度方向）
     */
    public Vec3 getTangentVector() {
        if (currentVelocity.lengthSqr() == 0) {
            return Vec3.ZERO; // 零速度时返回零向量
        }
        return currentVelocity.normalize();
    }

    /**
     * 计算从当前朝向旋转到切线方向的四元数
     * @param upDirection 参考上方向（用于确定旋转平面）
     * @param alpha 插值因子（0~1）
     */
    public Quaternionf calculateRotationToTangent(Vec3 upDirection, float alpha) {
        // 获取目标方向
        Vec3 tangent = getTangentVector();
        if (tangent.equals(Vec3.ZERO)) {
            return new Quaternionf(currentRotate); // 无速度时保持原旋转
        }

        // 将Minecraft的Vec3转换为JOML的Vector3f（注意坐标系差异）
        org.joml.Vector3f jomlTangent = new org.joml.Vector3f(
                (float) tangent.x,
                (float) tangent.y,
                (float) tangent.z
        );
        org.joml.Vector3f jomlUp = new org.joml.Vector3f(
                (float) upDirection.x,
                (float) upDirection.y,
                (float) upDirection.z
        );

        // 创建目标旋转：使物体的正前方向（假设为负Z轴）对齐切线方向
        Quaternionf targetRotation = new Quaternionf()
                .lookAlong(jomlTangent, jomlUp); // 使用lookAlong自动计算旋转

        // 使用slerp平滑过渡到目标旋转
        return currentRotate.slerp(targetRotation, alpha, new Quaternionf());
    }


    public  void getUpdated(Vec3 pos, Vec3 velocity, Vec3 acceleration, float tickDelta) {
        float deltaTime = PhysicsEngine.getDeltaTime(tickDelta);
        // 计算在当前时间间隔内的位移
        Vec3 displacement = velocity.multiply(PhysicsEngine.every(deltaTime));

        // 如果有加速度，计算由加速度引起的额外位移
        if (!acceleration.equals(Vec3.ZERO)) {
            // 使用公式 s = ut + 0.5at^2 来计算由加速度引起的位移
            Vec3 accelerationDisplacement = acceleration.multiply(PhysicsEngine.every(0.5 * deltaTime * deltaTime));
            displacement = displacement.add(accelerationDisplacement);

            // 更新速度，使用公式 v = u + at
            currentVelocity = velocity.add(acceleration.multiply(PhysicsEngine.every(deltaTime)));
        }

        // 更新位置
        currentPos =  pos.add(displacement);
    }

    public void physicsObjTick(float tickDelta) {
        if (mass != 0) {
            // 基础物理更新
            getUpdated(currentPos, currentVelocity,
                    PhysicsEngine.getAcceleration(currentForce, mass), tickDelta);

            // 计算并应用旋转（示例参数）
            if (!currentVelocity.equals(Vec3.ZERO)) {
                Vec3 up = new Vec3(0, 1, 0); // 使用世界Y轴作为参考上方向
                float rotationSpeed = 0.2f; // 控制旋转平滑度的插值因子

                // 计算插值后的旋转
                Quaternionf newRotation = calculateRotationToTangent(up, rotationSpeed);

                // 更新当前旋转
                currentRotate.set(newRotation);
            }
        }
    }

    public void setDisabled(boolean disabled) {
        this.disabled = disabled;
    }
    public boolean getDisabled() {
        return this.disabled;
    }
}
